#include "DiagMatrix.h"

#include <math.h>

DiagMatrix::DiagMatrix(void) :
	matG()
{
	LoadIdentity();
}

bool DiagMatrix::ApplyGivensRotation()
{
	// calculating GtAG
	float G[16];
	float Gt[16];
	matG.GetMatrix( G );
	matG.GetMatrixT( Gt );
	
	VMatrix GtAG;
	GtAG.LoadMatrix( Gt );
	GtAG.MultMatrix( &stack[STACK_CURRENT] );
	GtAG.MultMatrix( G );

	// checking if matrix has been diagonalized already
	bool jacobiIsOver = true;
	for( int j = 0; j < 16; j++ )
	{
		if( j != 0 && j != 5 && j != 10 && j != 15 )
		{
			int l = j % 4;
			int c = ( j - l ) / 4;
			if( abs( GtAG.Get( l+1, c+1 ) ) > 0.0001 )
			{
				jacobiIsOver = false;
				break;
			}
		}
	}	
	if( jacobiIsOver )
		return true;
	
	// if there's still some diagonalization to do,
	// calculate pivot i (row) and j (column)
	int i = -1;
	int j = -1;
	float pivot = -1.0f;

	for( int k = 0; k < 16; k++ )
	{
		int l = k % 4;
		int c = ( k - l ) / 4;
		if( l != c )
		{
			float possiblePivotAbs = abs( GtAG.Get( l+1, c+1 ) );
			if( possiblePivotAbs > pivot )
			{
				i = l;
				j = c;
				pivot = possiblePivotAbs;
			}
		}
	}

	// calculating theta, angle of the Givens rotation
	float Aij = GtAG.Get(i+1,j+1);
	float Aii = GtAG.Get(i+1,i+1);
	float Ajj = GtAG.Get(j+1,j+1);

	float theta;
	if( abs( Ajj - Aii ) < 0.0001 )
	{
		theta = PI / 4.0f;
	}
	else
	{
		// since tan(2*theta) = ( 2 * Aij ) / ( Ajj - Aii )
		theta = atan( ( 2.0f * Aij ) / ( Ajj - Aii ) ) / 2.0f;
	}

	// calculating c and s
	float c = cos(theta);
	float s = sin(theta);

	// calculating Gi matrix
	float Gi[16];

	for( int u = 0; u < 16; u++ )
	{
		int line = u % 4;
		int column = ( u - line ) / 4;
		if( line == column && line != i && line != j )
		{
			Gi[ u ] = 1.0f;
		}
		else if( ( line == i && column == i ) || ( line == j && column == j ) )
		{
			Gi[ u ] = c;
		}
		else if( line == j && column == i )
		{
			Gi[ u ] = -s;
		}
		else if( line == i && column == j )
		{
			Gi[ u ] = s;
		}
		else
		{
			Gi[ u ] = 0.0f;
		}
	}

	// rotate!
	matG.MultMatrix( Gi );
	return false;
}

void DiagMatrix::SquareRoot()
{
	// applying givens rotations
	for( int i = 0; i < MAX_GIVENS_ROTATIONS; i++ )
	{
		if( ApplyGivensRotation() )
			break;
	}

	// now, our matrix should be diagonalized
	float G[16];
	float Gt[16];
	matG.GetMatrix( G );
	matG.GetMatrixT( Gt );
	
	// calculating GtAG
	VMatrix GtAG;
	GtAG.LoadMatrix( Gt );
	GtAG.MultMatrix( &stack[STACK_CURRENT] );
	GtAG.MultMatrix( G );

	float rawGtAG[16];
	GtAG.GetMatrix( rawGtAG );

	// let's find its square root!
	for( int i = 0; i < 16; i++ )
	{
		if( i != 0 && i != 5 && i != 10 && i != 15 )
		{
			rawGtAG[ i ] = 0.0f;
		}
		else
		{
			if( rawGtAG[ i ] <= 0.0 )
			{
				rawGtAG[ i ] = 0.0f;
			}
			else
			{
				// squaring the diagonal elements
				rawGtAG[ i ] = sqrt( rawGtAG[ i ] );
			}
		}
	}

	// we multiply them back, so to have the composed matrix again
	VMatrix matSquareRoot;
	matSquareRoot.LoadMatrix( G );
	matSquareRoot.MultMatrix( rawGtAG );
	matSquareRoot.MultMatrix( Gt );

	float rawSquareRoot[ 16 ];
	matSquareRoot.GetMatrix( rawSquareRoot );
	LoadMatrix( rawSquareRoot );
}